/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "Renderer.h"
#include <GL/gl.h>
#include <GL/glext.h>
#include <math.h>
#include <fstream>
#include <iostream>
#include <exception>
#include <time.h>
#include "Quaternion.h"
#include "Drawable.h"

using std::map;
using std::vector;
using std::ios;
using std::string;

vector<Renderer*> Renderer::renderers;
clock_t Renderer::currTime;
int Renderer::frames;
int Renderer::win;
GLuint Renderer::grayscaleProgram, Renderer::grayscaleShader;
World *Renderer::root;
map<Entity*, GLuint> Renderer::displayLists;
void(*Renderer::stepFunction)(void);
Camera* Renderer::offscreenCamera;

int Renderer::buttons[5] = { GLUT_UP, GLUT_UP, GLUT_UP, GLUT_UP, GLUT_UP };
int Renderer::cur_x = 0;
int Renderer::cur_y = 0;

void Renderer::init(int argc, char **argv, World *world, void(*stepFunction)(void)) {
	Renderer::stepFunction = stepFunction;
	
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);

	glutInitWindowSize(640,480);
	win = glutCreateWindow("Incub");
	setShaders();
	glClearColor(0.0,0.0,0.0,0.0);
	
	// first renderer is for on-screen user controllable camera only
	offscreenCamera = new Camera();
	root = world;
	Renderer::addInstance(offscreenCamera);
	vector<Entity*>& entities = world->getEntities();
	for(vector<Entity*>::iterator entity = entities.begin(); entity != entities.end(); entity++) {
		activateEntity(*entity);
	}
}

void useMaterial(Material* mat) {
	GLfloat material_diffuse[] = { mat->getDiffuse().r, mat->getDiffuse().g, mat->getDiffuse().b, 1 };
	GLfloat material_specular[] = { mat->getSpecular().r, mat->getSpecular().g, mat->getSpecular().b, 1 };
	GLfloat material_ambient[] = { mat->getAmbient().r, mat->getAmbient().g, mat->getAmbient().b, 1 };
	GLfloat material_emission[] = { mat->getEmission().r, mat->getEmission().g, mat->getEmission().b, 1 };
	GLfloat material_shininess[] = { mat->getShininess() };
	glMaterialfv(GL_FRONT, GL_DIFFUSE, material_diffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR, material_specular);
	glMaterialfv(GL_FRONT, GL_AMBIENT, material_ambient);
	glMaterialfv(GL_FRONT, GL_EMISSION, material_emission);
	glMaterialfv(GL_FRONT, GL_SHININESS, material_shininess);
}

void transform(Transformable *trans) {
	static GLdouble M[16];
	static double R[9];
	trans->getRotation().getMatrix(R);
    M[0]  = R[0]; M[1]  = R[3]; M[2]  = R[6];  M[3]  = 0;
    M[4]  = R[1]; M[5]  = R[4]; M[6]  = R[7];  M[7]  = 0;
    M[8]  = R[2]; M[9]  = R[5]; M[10] = R[8]; M[11]  = 0;
    M[12] = trans->getX(); M[13] = trans->getY(); M[14] = trans->getZ();  M[15] = 1;
    glMultMatrixd(M);
}

void Renderer::activateJoint(Joint* joint) {
	Entity *entity = joint->getEntity();
	activateEntity(entity);
}

void Renderer::activateSensor(Sensor* sensor) {
	if(dynamic_cast<Camera*>(sensor)) {
		Camera *cam = dynamic_cast<Camera*>(sensor);
		Renderer::addInstance(cam);
	}
}

void Renderer::activateEntity(Entity* entity) {
	displayLists[entity] = glGenLists(1);
	glNewList(displayLists[entity],GL_COMPILE);
	vector<Shape*>& shapes = entity->getShapes();
	for(vector<Shape*>::iterator shape = entity->getShapes().begin(); shape != shapes.end(); shape++) {
		useMaterial((*shape)->getMaterial());
		glPushMatrix();

		transform(*shape);
		dynamic_cast<Drawable*>(*shape)->draw();

		glPopMatrix();
	}
	glEndList();
		
	vector<Sensor*>& sensors = entity->getSensors();
	vector<Joint*>& joints = entity->getJoints();

	for(unsigned i = 0; i < sensors.size(); i++) {
		activateSensor(sensors[i]);
	}
	for(unsigned i = 0; i != joints.size(); i++) {
		activateJoint(joints[i]);
	}
}

static void CheckError(int line) {
   GLenum err = glGetError();
   if (err) {
      printf("GL Error 0x%x at line %d\n", (int) err, line);
   }
}

void Renderer::setShaders() {
	grayscaleShader = glCreateShader(GL_FRAGMENT_SHADER);	
	const char *source = "void main() { gl_FragColor.a = dot(gl_Color.rgb, vec3(0.3, 0.59, 0.11)); }";
	glShaderSource(grayscaleShader, 1, &source ,NULL);
	glCompileShader(grayscaleShader);
	CheckError(__LINE__);
	grayscaleProgram = glCreateProgram();
	CheckError(__LINE__);
	glAttachShader(grayscaleProgram,grayscaleShader);
	CheckError(__LINE__);
	glLinkProgram(grayscaleProgram);
	CheckError(__LINE__);
	glValidateProgram(grayscaleProgram);
	CheckError(__LINE__);
}

void Renderer::addInstance(Camera *cam) {
	Renderer* r = new Renderer(cam);
	renderers.push_back(r);
}

Renderer::Renderer(Camera *cam) {
	// initialize glut
	this->camera = cam;
	width = cam->getHorizontalResolution();
	height = cam->getVerticalResolution();
	
	glGenFramebuffersEXT(1, &framebuffer);
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, framebuffer);
	
	/* Make texture object/image */
	glGenTextures(1, &texture);
	glBindTexture(GL_TEXTURE_2D, texture);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
	
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_BASE_LEVEL, 0);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAX_LEVEL, 0);
	
	CheckError(__LINE__);
	glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_TEXTURE_2D, texture, 0);
	
	/* make depth depthbuffer */
	glGenRenderbuffersEXT(1, &depthbuffer);
	glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, depthbuffer);
	glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT, width, height);
	CheckError(__LINE__);
	
	/* attach depthbuffer to framebuffer */
	glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, depthbuffer);
	CheckError(__LINE__);
	
	GLenum status = glCheckFramebufferStatusEXT(GL_FRAMEBUFFER_EXT);
//	cout << "framebuffer status: " << status << endl;;
	if(status != GL_FRAMEBUFFER_COMPLETE_EXT) exit(1);
	/* bind regular framebuffer */
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
}

void cameraTransform(Transformable *trans) {
	static GLdouble M[16];
	static double R[9];
	(-trans->getRotation()).getMatrix(R);
    M[0]  = R[0]; M[1]  = R[3]; M[2]  = R[6];  M[3]  = 0;
    M[4]  = R[1]; M[5]  = R[4]; M[6]  = R[7];  M[7]  = 0;
    M[8]  = R[2]; M[9]  = R[5]; M[10] = R[8]; M[11]  = 0;
    M[12] = 0; M[13] = 0; M[14] = 0;  M[15] = 1;
    glMultMatrixd(M);
    glTranslatef(-trans->getX(),-trans->getY(),-trans->getZ());
}

void Renderer::renderEntity(Entity* entity) {
	glPushMatrix();
	transform(entity);

	glCallList(displayLists[entity]);
	CheckError(__LINE__);

	vector<Joint*>& joints = entity->getJoints();
	for(vector<Joint*>::iterator joint = joints.begin(); joint != joints.end(); joint++) {
		renderEntity((*joint)->getEntity());
	}
	glPopMatrix();
}

void drawCoords() {
	glColor3f(1.0, 0, 0);
	
	glBegin(GL_LINES);
	glVertex3f(-1.0f, 0.0f, 0.0f); // origin of the line
	glVertex3f(1.0f, 0.0f, 0.0f); // ending point of the line
	glEnd( );
	
	glBegin(GL_LINE_STRIP);
	glVertex3f(0.9f, 0.0f, -0.1f); // ending point of the line
	glVertex3f(1.0f, 0.0f, 0.0f); // ending point of the line
	glVertex3f(0.9f, 0.0f, 0.1f); // ending point of the line
	glEnd();

	glColor3f(0, 1.0, 0);
	
	glBegin(GL_LINES);
	glVertex3f(0.0f, 0.0f, -1.0f); // origin of the line
	glVertex3f(0.0f, 0.0f, 1.0f); // ending point of the line
	glEnd( );
	
	glBegin(GL_LINE_STRIP);
	glVertex3f(-0.1f, 0.0f, 0.9f); // ending point of the line
	glVertex3f(0.0f, 0.0f, 1.0f); // ending point of the line
	glVertex3f(0.1f, 0.0f, 0.9f); // ending point of the line
	glEnd();
	
	glColor3f(0, 0, 0);
}

void createLight(Light *light, GLenum light_num) {
	// Create light components
	GLfloat ambientLight[] = { light->getAmbient(), light->getAmbient(), light->getAmbient(), 1.0f };
	GLfloat diffuseLight[] = { light->getDiffuse(), light->getDiffuse(), light->getDiffuse(), 1.0f };
	GLfloat specularLight[] = { light->getSpecular(), light->getSpecular(), light->getSpecular(), 1.0f };
	GLfloat position[] = { light->getX(), light->getY(), light->getZ(), 1.0f };
	// Assign created components to GL_LIGHTn
	glLightfv(light_num, GL_AMBIENT, ambientLight);
	glLightfv(light_num, GL_DIFFUSE, diffuseLight);
	glLightfv(light_num, GL_SPECULAR, specularLight);
	glLightfv(light_num, GL_POSITION, position);
}

void Renderer::disp(void){
	Renderer *rend = *renderers.begin();

	glUseProgram(0);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
	gluPerspective(45,1.0*rend->width/rend->height,rend->camera->getZNear(),rend->camera->getZFar());
	CheckError(__LINE__);

	cameraTransform(rend->camera);
			
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
	CheckError(__LINE__);

	/* draw to texture image */
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, rend->framebuffer);
	CheckError(__LINE__);

	glViewport(0, 0, rend->width, rend->height);
	
	glClearColor(0.0,0.0,0.0,0.0);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	glEnable(GL_NORMALIZE);
	glEnable(GL_BLEND);
	CheckError(__LINE__);

	vector<Entity*>& entities = rend->root->getEntities();
	
	// prepare lights
	static const GLenum lights_array[] = { GL_LIGHT0, GL_LIGHT1, GL_LIGHT2, GL_LIGHT3, GL_LIGHT4, GL_LIGHT5, GL_LIGHT6, GL_LIGHT7 };
	static vector<GLenum> lights = vector<GLenum>(lights_array, lights_array + sizeof(lights_array)/sizeof(*lights_array));  

	vector<GLenum>::iterator light_num = lights.begin(); 
	for(vector<Entity*>::iterator entity = entities.begin(); entity != entities.end(); entity++) {
		if(dynamic_cast<Light*>(*entity)) {
			if(light_num == lights.end()) throw RendererException("Too many lights");
			glEnable(*light_num);
			Light *light = dynamic_cast<Light*>(*entity);
			createLight(light, *light_num);
			light_num++;
		}
	}

	glLoadIdentity();
	// draw something
	for(vector<Entity*>::iterator entity = entities.begin(); entity != entities.end(); entity++) {
		rend->renderEntity(*entity);
	}

	glDisable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);

	drawCoords();

	/* Bind normal framebuffer */
	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
	CheckError(__LINE__);

   
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluLookAt( 0, 0, 0, 0, 0, -1, 0, 1, 0 );
	CheckError(__LINE__);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glViewport(0, 0, rend->width, rend->height);
	CheckError(__LINE__);
	
	glClear(GL_COLOR_BUFFER_BIT);
	CheckError(__LINE__);
	
	glPushMatrix();
		glRotatef(0, 0, 1, 0);
		glEnable(GL_TEXTURE_2D);
		glBindTexture(GL_TEXTURE_2D, rend->texture);
		CheckError(__LINE__);

		glBegin(GL_POLYGON);
			glTexCoord2f(0, 0);
			glVertex2f(-1, -1);
			glTexCoord2f(1, 0);
			glVertex2f(1, -1);
			glTexCoord2f(1, 1);
			glVertex2f(1, 1);
			glTexCoord2f(0, 1);
			glVertex2f(-1, 1);
		glEnd();
		CheckError(__LINE__);
		glDisable(GL_TEXTURE_2D);
	glPopMatrix();
	CheckError(__LINE__);

	glutSwapBuffers();
	CheckError(__LINE__);
}

void Renderer::renderOffscreen(void){
	for(vector<Renderer*>::iterator it = renderers.begin()+1; it != renderers.end(); it++) {
		Renderer *rend = *it;

		if(rend->camera->getType() == Camera::MONO) {
			glUseProgram(rend->grayscaleProgram);
			CheckError(__LINE__);
		}

	    glMatrixMode(GL_PROJECTION);
	    glLoadIdentity();
		gluPerspective(45,1.0*rend->width/rend->height,rend->camera->getZNear(),rend->camera->getZFar());
		CheckError(__LINE__);
	
		cameraTransform(rend->camera);
		Entity *parent = rend->camera->getParentEntity();
		do {
			cameraTransform(parent);
			if(parent->getParentJoint() == NULL) break;
			else parent = parent->getParentJoint()->getParentEntity();
		} while(parent != NULL);
				
	    glMatrixMode(GL_MODELVIEW);
	    glLoadIdentity();
		CheckError(__LINE__);
	
		/* draw to texture image */
		glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, rend->framebuffer);
		CheckError(__LINE__);
	
		glViewport(0, 0, rend->width, rend->height);
		
		glClearColor(0.0,0.0,0.0,0.0);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glEnable(GL_DEPTH_TEST);
		glEnable(GL_LIGHTING);
		glEnable(GL_NORMALIZE);
		CheckError(__LINE__);
	
		vector<Entity*>& entities = rend->root->getEntities();
		
		// prepare lights
		static const GLenum lights_array[] = { GL_LIGHT0, GL_LIGHT1, GL_LIGHT2, GL_LIGHT3, GL_LIGHT4, GL_LIGHT5, GL_LIGHT6, GL_LIGHT7 };
		static vector<GLenum> lights = vector<GLenum>(lights_array, lights_array + sizeof(lights_array)/sizeof(*lights_array));  
	
		vector<GLenum>::iterator light_num = lights.begin(); 
		for(vector<Entity*>::iterator entity = entities.begin(); entity != entities.end(); entity++) {
			if(dynamic_cast<Light*>(*entity)) {
				if(light_num == lights.end()) throw RendererException("Too many lights");
				glEnable(*light_num);
				Light *light = dynamic_cast<Light*>(*entity);
				createLight(light, *light_num);
				light_num++;
			}
		}

		glLoadIdentity();
		// draw something
		for(vector<Entity*>::iterator entity = entities.begin(); entity != entities.end(); entity++) {
			rend->renderEntity(*entity);
		}
	
		glDisable(GL_DEPTH_TEST);
		glDisable(GL_LIGHTING);
	
		glUseProgram(0);

		glBindTexture(GL_TEXTURE_2D, rend->texture);
		
		if(rend->camera->getType() == Camera::MONO) {
			glGetTexImage(GL_TEXTURE_2D, 0, GL_ALPHA, GL_UNSIGNED_BYTE, rend->camera->getBuffer());
		} else if(rend->camera->getType() == Camera::COLOR) {
			glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, rend->camera->getBuffer());
		} else if(rend->camera->getType() == Camera::DEPTH) {
			glReadPixels(0, 0, rend->width, rend->height, GL_DEPTH_COMPONENT, GL_FLOAT, rend->camera->getBuffer());
		}
	}
}

void Renderer::keyb(unsigned char key, int x, int y){
	if(key == 'q'){
		std::cout << "Got q,so quitting " << std::endl;
		throw ExitException();
	}
}

void Renderer::cleanup() {
	delete offscreenCamera;
	for(vector<Renderer*>::iterator it = renderers.begin(); it != renderers.end(); it++) {
    	Renderer* rend = *it;
    	delete rend;
	}
	glDetachShader(grayscaleProgram, grayscaleShader);
	glDeleteProgram(grayscaleProgram);
	glDeleteShader(grayscaleShader);
	glutDestroyWindow(win);
}

void Renderer::idle() {
	time_t clockDiff = clock() - currTime;
	if(clockDiff / CLOCKS_PER_SEC > 5) {
		float fps = (1.0*frames / clockDiff) * CLOCKS_PER_SEC;
		std::cout << "fps: " << fps << std::endl;
		currTime = clock();
		frames = 1;
	}
	renderOffscreen();
	glutPostRedisplay();
	frames++;
}

void Renderer::mouse(int button, int state, int x, int y) {
	buttons[button] = state;
	cur_x = x;
	cur_y = y; 
}

void Renderer::motion (int x, int y) {
	const double movFactor = 20.0;
	const double rotFactor = 200.0;
	bool changed = false;
	Camera* camera = renderers[0]->camera;

	// process UI events
	if (buttons[GLUT_LEFT_BUTTON] == GLUT_DOWN) {

		Quaternion qx(0.0, 1.0, 0.0, (x - cur_x)/rotFactor);
		Quaternion qy(1.0, 0.0, 0.0, (y - cur_y)/rotFactor);

		Quaternion rotated = qy * (-camera->getRotation()) * qx;

		camera->setRotation(-rotated);
		changed = true;
	}
  
	if (buttons[GLUT_MIDDLE_BUTTON] == GLUT_DOWN) {
		// track camera forward and back using middle mouse button
		double z[] = { 0, 0, (y - cur_y)/movFactor };
		camera->getRotation().rotate(z);
		
		camera->setPosition(camera->getX() + z[0], camera->getY() + z[1], camera->getZ() + z[2]);
		changed = true;
	}
  
	if (buttons[GLUT_RIGHT_BUTTON] == GLUT_DOWN) {
		// pan camera using right mouse button
		double v[] = { (x - cur_x)/movFactor, (cur_y - y)/movFactor, 0 };
		camera->getRotation().rotate(v);
		
		camera->setPosition(camera->getX()+v[0], camera->getY()+ v[1], camera->getZ() + v[2]);
		changed = true;
	}

	if (changed) glutPostRedisplay(); 

	cur_x = x;
	cur_y = y; 
}

void Renderer::render_all() {
	currTime = clock();
	frames = 1;

	glutDisplayFunc(Renderer::disp);
    glutKeyboardFunc(Renderer::keyb);
    glutIdleFunc(Renderer::stepFunction);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();
}

Renderer::~Renderer() {
}
